// SPDX-License-Identifier:	GPL-2.0+
#include <common.h>
#include <dm.h>
#include <clk.h>
#include <clk-uclass.h>
#include <asm/io.h>
#include <div64.h>

#include "ls7a_init_lib.h"
#include "ls7a_mc_config.h"

#ifdef DEBUG
#define DEBUG_ENABLE_LOONGSON
#define ls7a_debug printf
#else
#undef DEBUG_ENABLE_LOONGSON
#define ls7a_debug(fmt, args...)
#endif

u8 uDimmOrder[9] = {0,1,2,3,4,5,6,7,8};
u8 rDimmOrder[9] = {8,3,2,1,0,4,5,6,7};

//write_mc_reg
void WriteMcReg8(u64 McRegsBase, u32 Offset, u8 Data)
{
	Writeb(McRegsBase + Offset, Data);
}

u8 ReadMcReg8(u64 McRegsBase, u32 Offset)
{
	return Readb(McRegsBase + Offset);
}

void WriteMcReg16(u64 McRegsBase, u32 Offset, u16 Data)
{
	Write16(McRegsBase + Offset, Data);
}

u16 ReadMcReg16(u64 McRegsBase, u32 Offset)
{
	return Read16(McRegsBase + Offset);
}

void WriteMcReg32(u64 McRegsBase, u32 Offset, u32 Data)
{
	Writel(McRegsBase + Offset, Data);
}

u32 ReadMcReg32(u64 McRegsBase, u32 Offset)
{
	return Readl(McRegsBase + Offset);
}

void WriteMcReg64(u64 McRegsBase, u32 Offset, u64 Data)
{
	Writeq(McRegsBase + Offset, Data);
}

u64 ReadMcReg64(u64 McRegsBase, u32 Offset)
{
	return Readq(McRegsBase + Offset);
}

void WaitInitDone(u64 McRegsBase)
{
	u8 Val8;
	do {
		Val8 = Readb(McRegsBase + DDR3_DRAM_INIT_OFFSET);
	} while (Val8 == 0);
}

#if 0
void ddr_dll_bypass(
  IN u32 Offset
)
{
  u32 Val32;
  Val32 = *(volatile u32 *)(McRegsBase + Offset);
  Val32 *=pm_dll_value;
  Val32 = Val32 >> 7;
  Val32 |=0x80;
  return RETURN_SUCCESS;
}
#endif

void DLLWrDataSet(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);
	if (Val8 >= DLL_WRDQ_SUB) {
		Val8 -= DLL_WRDQ_SUB;
	} else {
		Val8 = Val8 + 0x80 - DLL_WRDQ_SUB;
	}
	WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET + SliceNum * 0x20, Val8);
}

void WrdqLtHalfSet(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET + SliceNum * 0x20);
//	if (SliceNum==0)
//		ls7a_debug("DllWrdq=: %x\n", Val8);
	if (Val8 >= WRDQ_LT_HALF_STD) {
		Val8 = 0;
	} else {
		Val8 = 1;
	}
//	if (SliceNum==0)
//		ls7a_debug("wrdq_lt_half=: %x\n", Val8);

	WriteMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + SliceNum * 0x20, Val8);
}


void WrdqsLtHalfSet(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);
	if (Val8 >= WRDQS_LT_HALF_STD) {
		Val8 = 0;
	} else {
		Val8 = 1;
	}

	WriteMcReg8(McRegsBase, WRDQS_LT_HALF_OFFSET + SliceNum * 0x20, Val8);
}


void WlvlGet0(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	u8 i = 0;
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
	} while(Val8 == 0);

	while ((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & WLVL_CHECK_BIT) {
		Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);
		Val8 = (Val8 + 1) & 0x7f;
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, Val8);
		#ifdef DQ_CHANGE_WITH_DQS
		DLLWrDataSet(McRegsBase, SliceNum);
		WrdqLtHalfSet(McRegsBase, SliceNum);
		WrdqsLtHalfSet(McRegsBase, SliceNum);
		#endif
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

//		ls7a_debug("write leveling: slice %d searching 0\r\n", SliceNum);

		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while(Val8 == 0);
	}

	// filter the 0 to 1 glitch

	while (i<0x38) {
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while(Val8 == 0);

		while ((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & WLVL_CHECK_BIT) {
			Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);
			Val8 = (Val8 + 1) & 0x7f;
			WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, Val8);
			#ifdef DQ_CHANGE_WITH_DQS
			DLLWrDataSet(McRegsBase, SliceNum);
			WrdqLtHalfSet(McRegsBase, SliceNum);
			WrdqsLtHalfSet(McRegsBase, SliceNum);
			#endif
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
			do {
				Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
			} while(Val8 == 0);

			i = 0;
		}
		i++;
	}
//	return RETURN_SUCCESS;
}

void WlvlGet1(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	u8 i = 0;
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
	} while(Val8 == 0);

	while (!((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & WLVL_CHECK_BIT)) {
		Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);
		Val8 = (Val8 + 1) & 0x7f;
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, Val8);
		#ifdef DQ_CHANGE_WITH_DQS
		DLLWrDataSet(McRegsBase, SliceNum);
		WrdqLtHalfSet(McRegsBase, SliceNum);
		WrdqsLtHalfSet(McRegsBase, SliceNum);
		#endif
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

		//ls7a_debug("write leveling: slice %d searching 1\r\n", SliceNum);
		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while(Val8 == 0);
	}

	// filter the 1 to 0 glitch

	while (i<WLVL_FILTER_LEN) {
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while(Val8 == 0);

		while (!((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & WLVL_CHECK_BIT)) {
			Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum*0x20);
			Val8 = (Val8 + 1) & 0x7f;
			WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, Val8);
			#ifdef DQ_CHANGE_WITH_DQS
			DLLWrDataSet(McRegsBase, SliceNum);
			WrdqLtHalfSet(McRegsBase, SliceNum);
			WrdqsLtHalfSet(McRegsBase, SliceNum);
			#endif
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
			do {
				Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
			} while(Val8 == 0);

			i = 0;
		}
		i++;
	}

	//set back
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);
	Val8 = ((Val8 > WLVL_FILTER_LEN) ? Val8 - WLVL_FILTER_LEN : Val8 + 0x80 - WLVL_FILTER_LEN) & 0x7f;
	WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, Val8);
	DLLWrDataSet(McRegsBase, SliceNum);
	WrdqLtHalfSet(McRegsBase, SliceNum);
	WrdqsLtHalfSet(McRegsBase, SliceNum);
}

void DLLAdj(u64 McRegsBase, u32 SliceNum)
{
	u8 DllWrdqs;
	u8 DllWrdq;
	DllWrdqs = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20);

	if (DllWrdqs >= DLL_WRDQ_SUB) {
		DllWrdq = DllWrdqs - DLL_WRDQ_SUB;
	} else {
		DllWrdq = DllWrdqs + 0x80 - DLL_WRDQ_SUB;
	}

	if (DllWrdqs >= 0x0 && DllWrdqs < DLL_ADJ_RANGE) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, 0x8);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20, (0x08-DLL_WRDQ_SUB)&0x7f);
	} else if (DllWrdqs >= 0x40-DLL_ADJ_RANGE && DllWrdqs < 0x40) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, 0x38);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20, (0x38-DLL_WRDQ_SUB)&0x7f);
	} else if (DllWrdqs >= 0x40 && DllWrdqs < 0x40+DLL_ADJ_RANGE) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, 0x48);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20, (0x48-DLL_WRDQ_SUB)&0x7f);
	} else if (DllWrdqs >= 0x80-DLL_ADJ_RANGE && DllWrdqs <= 0x7f) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, 0x78);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20, (0x78-DLL_WRDQ_SUB)&0x7f);
	}

	if (DllWrdq >= 0x0 && DllWrdq < DLL_ADJ_RANGE) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, (0x08+DLL_WRDQ_SUB)&0x7f);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20,  0x08);
	} else if (DllWrdq >= 0x40-DLL_ADJ_RANGE && DllWrdq < 0x40) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, (0x38+DLL_WRDQ_SUB)&0x7f);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20,  0x38);
	} else if (DllWrdq >= 0x40 && DllWrdq < 0x40+DLL_ADJ_RANGE) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, (0x48+DLL_WRDQ_SUB)&0x7f);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20,  0x48);
	} else if (DllWrdq >= 0x80-DLL_ADJ_RANGE && DllWrdq <= 0x7f) {
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET + SliceNum * 0x20, (0x78+DLL_WRDQ_SUB)&0x7f);
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET  + SliceNum * 0x20,  0x78);
	}
}

void RdOeSub(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	Val8 = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+SliceNum*0x20);
	Val8-= 1;
	Val8 &= 0x3;
	WriteMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+SliceNum*0x20, Val8);
	WriteMcReg8(McRegsBase, RDOE_END_OFFSET+SliceNum*0x20, Val8);
	WriteMcReg8(McRegsBase, RDODT_BEGIN_OFFSET+SliceNum*0x20, Val8);
	WriteMcReg8(McRegsBase, RDODT_END_OFFSET+SliceNum*0x20, Val8);

	if (Val8 == 0x3) {
		ls7a_debug("ERROR: slice %01d RdOeSub underflow\n", SliceNum);
//		WriteMcReg8(McRegsBase, tRDDATA_OFFSET, ReadMcReg8(McRegsBase, tRDDATA_OFFSET)-1);
	}
}

void RdOeAdd(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	Val8 = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+SliceNum*0x20);
	Val8+= 1;
	Val8 &= 0x3;
	WriteMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+SliceNum*0x20, Val8);
	WriteMcReg8(McRegsBase, RDOE_END_OFFSET+SliceNum*0x20, Val8);
	WriteMcReg8(McRegsBase, RDODT_BEGIN_OFFSET+SliceNum*0x20, Val8);
	WriteMcReg8(McRegsBase, RDODT_END_OFFSET+SliceNum*0x20, Val8);

	if (Val8 == 0x0) {
		ls7a_debug("ERROR: slice %01d RdOeAdd overflow\n", SliceNum);
//		WriteMcReg8(McRegsBase, tRDDATA_OFFSET, ReadMcReg8(McRegsBase, tRDDATA_OFFSET)+1);
	}
}

void DLLGateAdd(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	u16 k;

	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20);
	Val8 = (Val8 + 1) & 0x7f;
	WriteMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20, Val8);
	#if 0
	ls7a_debug("dll_gate = %x\n", Val8);
	#endif
	if (Val8 == 0x00) {
//		RdOeAdd(SliceNum);
		DDR_tRDDATA_ADD;
		#if 0
		ls7a_debug("ERROR: slice %01d RdOeAdd\n", SliceNum);
		#endif
		for (k=0;k<512;k++) { //sync code
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
		}
	}
}

void GlvlGet0(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	u8 i = 0;
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
	} while (Val8 == 0);

	while ((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & GLVL_CHECK_BIT) {
		DLLGateAdd(McRegsBase, SliceNum);
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

//		ls7a_debug("gate leveling: slice %d searching 0\r\n", SliceNum);

		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while (Val8 == 0);
	}
	// filter the 0 to 1 glitch

	while (i<0x10) {

		DLLGateAdd(McRegsBase, SliceNum);

		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while (Val8 == 0);

		while ((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & GLVL_CHECK_BIT) {
			DLLGateAdd(McRegsBase, SliceNum);

			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
			do {
				Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
			} while(Val8 == 0);

			i = 0;
		}
		i++;
	}
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20);
	ls7a_debug("gate leveling: slice %d found 0, dll_gate = 0x%02x\r\n", SliceNum, Val8);
//	return RETURN_SUCCESS;
}

void GlvlGet1(u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	u8 i = 0;
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
	} while(Val8 == 0);

	while (!((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & GLVL_CHECK_BIT)) {
		DLLGateAdd(McRegsBase, SliceNum);

		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

		#ifdef DDR_DEBUG
		ls7a_debug("gate leveling: slice %d searching 1\r\n", SliceNum);
		#endif

		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while(Val8 == 0);
	}

  	// filter the 1 to 0 glitch

	while(i<GLVL_FILTER_LEN) {
		DLLGateAdd(McRegsBase, SliceNum);

		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0

		do {
			Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
		} while(Val8 == 0);

		while (!((ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum)) & GLVL_CHECK_BIT)) {
			DLLGateAdd(McRegsBase, SliceNum);

			#ifdef DDR_DEBUG
			ls7a_debug("gate leveling: slice %d filter 1 to 0\r\n", SliceNum);
			#endif

			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
			do {
				Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
			} while(Val8 == 0);

			i = 0;
		}
		i++;
	}

	//set back
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20);
	if (Val8 >= GLVL_FILTER_LEN) {
		Val8 = (Val8 - GLVL_FILTER_LEN) & 0x7f;
	} else {
		Val8 = (Val8 + 0x80 - GLVL_FILTER_LEN) & 0x7f;
		DDR_tRDDATA_SUB;
		//for(k=0;k<512;k++){ //sync code
		//    WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		//    WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
		//}
	}
	WriteMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20, Val8);

	#ifdef DEBUG_ENABLE_LOONGSON
	//def DDR_DEBUG
	u8 tRDDATA = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
	ls7a_debug("gate leveling: slice %d found 1, dll_gate = 0x%02lx, tRDDATA = 0x%02lx\r\n", SliceNum, Val8, tRDDATA);
	#endif
}

void RddqsLtHalfSet (u64 McRegsBase, u32 SliceNum)
{
	u8 Val8_0;
	u8 Val8_1;
	u8 Val8;
	Val8_0 = ReadMcReg8(McRegsBase, DDR3_DLL_WRDQ_OFFSET + SliceNum * 0x20);
	Val8_1 = ReadMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20);
	Val8   = (Val8_0 + Val8_1) & 0x7f;
	if ((Val8 >= RDDQS_LT_HALF_STD1) || (Val8 < RDDQS_LT_HALF_STD2)) {
		Val8 = 1;
	} else {
		Val8 = 0;
	}

	WriteMcReg8(McRegsBase, RDDQS_LT_HALF_OFFSET + SliceNum * 0x20, Val8);
}

void WrdqclkdelaySet (u64 McRegsBase, u32 ModuleType, u8 EccEnable)
{
//	u8 rdimm_has_found = 0;
	s32 i,j;
	if (ModuleType == UDIMM || ModuleType == SODIMM){
		//first set all wrdq_clkdelay to 0x0
		for (i=0;i<8+EccEnable;i++) {
			WriteMcReg8(McRegsBase, WRDQ_CLKDELAY_OFFSET+uDimmOrder[i] * 0x20, 0x0);
		}

		for (i=0;i<7+EccEnable;i++) {
			u8 last_value = ReadMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + uDimmOrder[i] * 0x20);
			u8 value = ReadMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + uDimmOrder[i+1] * 0x20);

		//	ls7a_debug("slice %d: last_value = %x, value = %x\r\n", i, last_value, value);

			if (i==0 && last_value == 1) {
			//	ls7a_debug("slice %d: SUB", i);
				u8 Val8;
				Val8 = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
				Val8 -= 1;
				WriteMcReg8(McRegsBase, tRDDATA_OFFSET, Val8);

				Val8 = ReadMcReg8(McRegsBase, tPHY_WRLAT_OFFSET);
				Val8 -= 1;
				WriteMcReg8(McRegsBase, tPHY_WRLAT_OFFSET, Val8);
			}

			if (last_value == 1 && value == 0) {
				for (j=i+1;j<8+EccEnable;j++) {
					WriteMcReg8(McRegsBase, WRDQ_CLKDELAY_OFFSET+uDimmOrder[j] * 0x20, 0x1);
				}
				break;
			}
		}
	} else if (ModuleType == RDIMM) {
		//first set all wrdq_clkdelay to 0x0
		u8 already_sub = 0;
		for (i=0;i<8+EccEnable;i++) {
			WriteMcReg8(McRegsBase, WRDQ_CLKDELAY_OFFSET+rDimmOrder[i] * 0x20, 0x0);
		}

		for (i=1-EccEnable;i<5;i++) {
			u8 last_value = ReadMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + rDimmOrder[i] * 0x20);
			u8 value = ReadMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + rDimmOrder[i+1] * 0x20);
//			ls7a_debug("last_value = %x, value = %x\n", last_value, value);
			if (i==1-EccEnable && last_value == 1) {
				u8 Val8;
				Val8 = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
				Val8 -= 1;
				WriteMcReg8(McRegsBase, tRDDATA_OFFSET, Val8);

				Val8 = ReadMcReg8(McRegsBase, tPHY_WRLAT_OFFSET);
				Val8 -= 1;
				WriteMcReg8(McRegsBase, tPHY_WRLAT_OFFSET, Val8);
				already_sub = 1;
			}

			if (last_value == 1 && value == 0) {
				for (j=i+1;j<5;j++) {
					WriteMcReg8(McRegsBase, WRDQ_CLKDELAY_OFFSET+rDimmOrder[j] * 0x20, 0x1);
				}
				break;
			}
		}

		for (i = 5; i < 8; i++) {
			u8 last_value = ReadMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + rDimmOrder[i] * 0x20);
			u8 value = ReadMcReg8(McRegsBase, WRDQ_LT_HALF_OFFSET + rDimmOrder[i+1] * 0x20);
//			ls7a_debug("last_value = %x, value = %x\n", last_value, value);

			if (i==5 && last_value == 1) {
				u8 Val8;
				if (!already_sub) {
					Val8 = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
					Val8 -= 1;
					WriteMcReg8(McRegsBase, tRDDATA_OFFSET, Val8);

					Val8 = ReadMcReg8(McRegsBase, tPHY_WRLAT_OFFSET);
					Val8 -= 1;
					WriteMcReg8(McRegsBase, tPHY_WRLAT_OFFSET, Val8);
				}
			}

			if (last_value == 1 && value == 0) {
				for (j = i + 1; j < 9; j++) {
					WriteMcReg8(McRegsBase, WRDQ_CLKDELAY_OFFSET+rDimmOrder[j] * 0x20, 0x1);
				}
				break;
			}
		}
	}
}

void DLLGateSub (u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
	u16 k;
	Val8 = ReadMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20);
	if (Val8 >= DLL_GATE_SUB) {
		Val8 -= DLL_GATE_SUB;
	} else {
		Val8 = Val8 + 0x80 - DLL_GATE_SUB;

		//RdOeSub(SliceNum);
		DDR_tRDDATA_SUB;
		for(k=0;k<512;k++){ //sync code
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
			WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
		}
	}
	WriteMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET + SliceNum * 0x20, Val8);
}


void RddqsCntCheck (u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
//	u8 i = 0;

	u8 RddqsCnt1st = 0;
	u8 RddqsCnt2nd = 0;

	//first read;
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
	} while(Val8 == 0);

	RddqsCnt1st = (ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum) >> 2) & 0x7;

	//second read;
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
	WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
	} while(Val8 == 0);

	RddqsCnt2nd = (ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET + SliceNum) >> 2) & 0x7;

	if (((RddqsCnt2nd - RddqsCnt1st) & 0x7) != BURST_LENGTH/2) {
		ls7a_debug("Slice %d: rddqs counter check error, counter value is: %d\r\n\r\n", SliceNum, ((RddqsCnt2nd - RddqsCnt1st) & 0x7));
	} else {
		ls7a_debug("Slice %d: rddqs counter check pass\r\n\r\n", SliceNum);
	}

}


void RddqsPreambleCheck (u64 McRegsBase, u32 SliceNum)
{
	u8 Val8;
//	u8 Max=0;
	u8 j;
	u16 k;
//	u8 dll_gate[9];
	u8 dll_gate_ori;
//	u8 rd_oe_begin_ori;
//	u8 rd_oe_end_ori;
	u8 PreambleFound;
	u8 HasSub;

//	for (i=0;i<8+EccEnable;i++) {
//		dll_gate[i] = ReadMcReg8(McRegsBase, DLL_GATE_OFFSET+i*0x20);
//	}

//	//find the Max rd_oe_begin/end
//	for (i=0;i<8+EccEnable;i++) {
//		if (Max < ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20)) {
//			Max = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20);
//		}
//	}
//	ls7a_debug("Max = %d\r\n", Max);;
//
//	for (i=0;i<8+EccEnable;i++) {
//		Val8 = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20);
//		Val8 = Val8 + (3- Max);
//		ls7a_debug("rd_oe = %d\r\n", Val8);;
//		WriteMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20, Val8);
//		WriteMcReg8(McRegsBase, RDOE_END_OFFSET+i*0x20, Val8);
//	}
//
//	Val8 = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
//	Val8 = Val8 - (3 - Max);
//	WriteMcReg8(McRegsBase, tRDDATA_OFFSET, Val8);
//	ls7a_debug("trdData = %d\r\n", Val8);;

//	for (i=0;i<SliceNum;i++) {
	//save dll_gate
	dll_gate_ori = ReadMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET+SliceNum*0x20);
//	rd_oe_begin_ori = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+SliceNum*0x20);
//	rd_oe_end_ori   = ReadMcReg8(McRegsBase, RDOE_END_OFFSET+SliceNum*0x20);
	do {
		PreambleFound = 1;
		HasSub  = 0;
		for (j=0;j<PREAMBLE_LEN-0x10;j++) {
			u8 Sample5 = 0;
			Val8 = dll_gate_ori-0x10-j;
			Val8 = Val8 & 0x7f;
			WriteMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET+SliceNum*0x20, Val8);
			#ifdef DDR_DEBUG
			ls7a_debug("slice %d dll_gate = 0x%x\n", SliceNum, Val8);
			#endif
			if (Val8 == 0x7f) {
	//			RdOeSub(SliceNum);
				DDR_tRDDATA_SUB;
				HasSub = 1;

				for (k=0;k<512;k++) { //sync code
					WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
					WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
				}
			}

		//	for (k=0;k<10;k++) { //sync code
		//		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
		//		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
		//	}

			for (k=0;k<5;k++) {
				WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
				WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
				do {
					Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_DONE_OFFSET);
				} while(Val8 == 0);

				Sample5 = (Sample5 << 1) | (ReadMcReg8(McRegsBase, DDR3_LVL_RESP_OFFSET+SliceNum) & GLVL_CHECK_BIT & 0x1);
			}

			#ifdef DDR_DEBUG
			ls7a_debug("slice %d glvl_resp = 0x%x\n", SliceNum, Sample5);
			#endif
			if (Sample5 & 0x1f) {
				ls7a_debug("slice %d preamble check failed @ 0x%x\n", SliceNum, j);
				PreambleFound = 0;
				if (!HasSub) {
					DDR_tRDDATA_SUB;
				}
				break;
			}
			if (j==PREAMBLE_LEN-0x10-1) {
				ls7a_debug("slice %d preamble check pass\n", SliceNum);
			}
		}
	} while (!PreambleFound);

	//restore dll_gate
//	WriteMcReg8(McRegsBase, DLL_GATE_OFFSET+SliceNum*0x20, dll_gate_ori);
//	WriteMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+SliceNum*0x20, rd_oe_begin_ori);
//	WriteMcReg8(McRegsBase, RDOE_END_OFFSET+SliceNum*0x20, rd_oe_end_ori);
//	for (k=0;k<512;k++) { //sync code
//		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x1);//first set to 0x1
//		WriteMcReg8(McRegsBase, DDR3_LVL_REQ_OFFSET, 0x0);//then set to 0x0
//	}
	GlvlGet1(McRegsBase, SliceNum);
	RddqsLtHalfSet(McRegsBase, SliceNum);
	DLLGateSub(McRegsBase, SliceNum);
//	}
}
//leveling
void DDR3Leveling(u64 RegsBase, u64 NodeId, u8 ModuleType, u8 NumSlice, u8 RawCardVersion)
{
	int i = 0;
//	int j = 0;
//	u32 pm_dll_value;
//	u32 RegAddr;
	u8  Val8;
//	u64 Val64;
	u8  tRddataOri;
	u8  tRDDATA[NumSlice];
	u64 McRegsBase = RegsBase | (NodeId << 44);
	u8  Max = 0;
	u8  Min = 255;
	u8  EccEnable = 0;

//	ddr_dll_bypass(DLL_CK0_OFFSET);
//	ddr_dll_bypass(DLL_CK1_OFFSET);
//	ddr_dll_bypass(DLL_CK2_OFFSET);
//	ddr_dll_bypass(DLL_CK3_OFFSET);
	EccEnable = NumSlice & 0x1; //If enable Ecc, the NumSlice will be 9, otherwise it is 8;

	if (RawCardVersion == RAW_CARD_VERSION_F && ModuleType == SODIMM) {
		uDimmOrder[0]= 3;
		uDimmOrder[1]= 1;
		uDimmOrder[2]= 0;
		uDimmOrder[3]= 2;
		uDimmOrder[4]= 5;
		uDimmOrder[5]= 6;
		uDimmOrder[6]= 7;
		uDimmOrder[7]= 4;
		uDimmOrder[8]= 8;
	}

	WaitInitDone(McRegsBase);

	ls7a_debug("write leveling begin\r\n");
	//set all DllWrdqs to 0x0
	for (i=0;i<NumSlice;i++){
		WriteMcReg8(McRegsBase, DDR3_DLL_WRDQS_OFFSET+i*0x20, 0x0);
	}
	//set leveling_mode
	ls7a_debug("set leveling mode to be WRITE LEVELING\r\n");
	WriteMcReg8(McRegsBase, DDR3_LVL_MODE_OFFSET, 0x1);
	//
	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_READY_OFFSET);
	} while(Val8 == 0);

	ls7a_debug("write leveling ready\r\n");

	for (i=0;i<NumSlice;i++) {
		WlvlGet0(McRegsBase, i);
	}

	for (i=0;i<NumSlice;i++) {
		WlvlGet1(McRegsBase, i);
	}

	for (i=0;i<NumSlice;i++) {
		DLLAdj(McRegsBase, i);
	}


	WrdqclkdelaySet(McRegsBase, ModuleType, EccEnable);//UDIMM:00, RDIMM:01

	WriteMcReg8(McRegsBase, DDR3_LVL_MODE_OFFSET, 0x0);

	ls7a_debug("the parameter after write leveling is:\r\n");

//	for (i=0;i<(NumSlice)*4;i++) {
//		RegAddr = 0x20 + 0x8 * i;
//		Val64 = ReadMcReg64(McRegsBase, RegAddr);
//		ls7a_debug("%03x: %016lx\r\n", RegAddr, Val64);
//	}
#ifdef DEBUG_ENABLE_LOONGSON
	//def DDR_DEBUG
	u32 RegAddr;
	u64 Val64;
	for (i=0;i<MC_REGS_COUNT;i++) {
		RegAddr = 0x8 * i;
		Val64 = ReadMcReg64(McRegsBase, RegAddr);
		ls7a_debug("%03x: %016lx\r\n", RegAddr, Val64);
	}
	if (i%4 == 3) {
		ls7a_debug( "\r\n");
	}
#endif

	WriteMcReg8(McRegsBase, DDR3_LVL_MODE_OFFSET, 0x0);

	//reset init_start
	WriteMcReg8(McRegsBase, DDR3_INIT_START_OFFSET, 0x0);
	WriteMcReg8(McRegsBase, DDR3_INIT_START_OFFSET, 0x1);
	WaitInitDone(McRegsBase);

	//set all dll_gate to 0x0
	for (i=0;i<NumSlice;i++) {
		WriteMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET+i*0x20, 0x0);
	}

	//set leveling_mode
	ls7a_debug("set leveling mode to be Gate LEVELING\r\n");

	WriteMcReg8(McRegsBase, DDR3_LVL_MODE_OFFSET, 0x2);

	do {
		Val8 = ReadMcReg8(McRegsBase, DDR3_LVL_READY_OFFSET);
	} while(Val8 == 0);


	for (i=0;i<NumSlice;i++) {
		tRddataOri = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
		WriteMcReg8(McRegsBase, DDR3_DLL_GATE_OFFSET+i*0x20, 0x0);
		GlvlGet0(McRegsBase, i);

		//reset init_start
		WriteMcReg8(McRegsBase, DDR3_INIT_START_OFFSET, 0x0);
		WriteMcReg8(McRegsBase, DDR3_INIT_START_OFFSET, 0x1);
		WaitInitDone(McRegsBase);

		GlvlGet1(McRegsBase, i);
	//	DLLGateSub(i);
		RddqsPreambleCheck(McRegsBase, i);
		RddqsCntCheck(McRegsBase, i);
		tRDDATA[i] = ReadMcReg8(McRegsBase, tRDDATA_OFFSET);
		WriteMcReg8(McRegsBase, tRDDATA_OFFSET, tRddataOri);
	}

	//find the Max rd_oe_begin/end
	for (i=0;i<NumSlice;i++) {
		if (Max < tRDDATA[i]) {
			Max = tRDDATA[i];
		}
		if (Min > tRDDATA[i]) {
			Min = tRDDATA[i];
		}
	}

#ifdef DDR_DEBUG
	for (i=0;i<NumSlice;i++) {
		ls7a_debug("tRDDATA[%01d] = 0x%02lx\r\n", i, tRDDATA[i]);
	}
	ls7a_debug("Max = %01d, Min = %01d\r\n", Max, Min);
#endif

	if (Max-Min > 3) {
		ls7a_debug("ERROR: read gate window difference is too large\r\n");
	} else {
		WriteMcReg8(McRegsBase, tRDDATA_OFFSET, Min + ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET) - 1); //here assume all slice has same rd_oe_begin/end
		for (i=0;i<NumSlice;i++) {
			Val8 = tRDDATA[i] - Min + 1;
			WriteMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20, Val8);
			WriteMcReg8(McRegsBase, RDOE_END_OFFSET+i*0x20, Val8);

			Val8 = ReadMcReg8(McRegsBase, RDOE_START_OFFSET+i*0x20);
			if (Val8 > 0x1) {
				Val8 -= 0x2;
				WriteMcReg8(McRegsBase, RDODT_START_OFFSET+i*0x20, Val8);
				Val8 = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20);
				WriteMcReg8(McRegsBase, RDODT_BEGIN_OFFSET+i*0x20, Val8);
			} else {
				Val8 += 0x2;
				WriteMcReg8(McRegsBase, RDODT_START_OFFSET+i*0x20, Val8);
				Val8 = ReadMcReg8(McRegsBase, RDOE_BEGIN_OFFSET+i*0x20);
				Val8 -= 0x1;
				WriteMcReg8(McRegsBase, RDODT_BEGIN_OFFSET+i*0x20, Val8);
			}

			Val8 = ReadMcReg8(McRegsBase, RDOE_STOP_OFFSET+i*0x20);
			if (Val8 < 0x2) {
				Val8 += 0x2;
				WriteMcReg8(McRegsBase, RDODT_STOP_OFFSET+i*0x20, Val8);
				Val8 = ReadMcReg8(McRegsBase, RDOE_END_OFFSET+i*0x20);
				WriteMcReg8(McRegsBase, RDODT_END_OFFSET+i*0x20, Val8);
			} else {
				Val8 -= 0x2;
				WriteMcReg8(McRegsBase, RDODT_STOP_OFFSET+i*0x20, Val8);
				Val8 = ReadMcReg8(McRegsBase, RDOE_END_OFFSET+i*0x20);
				Val8 += 0x1;
				WriteMcReg8(McRegsBase, RDODT_END_OFFSET+i*0x20, Val8);
			}
		}
//      	WriteMcReg8(McRegsBase, RDODT_BEGIN_OFFSET+i*0x20, Val8);
//      	WriteMcReg8(McRegsBase, RDODT_END_OFFSET+i*0x20, Val8);
	}


	WriteMcReg8(McRegsBase, DDR3_LVL_MODE_OFFSET, 0x0);

	//reset init_start
	WriteMcReg8(McRegsBase, DDR3_INIT_START_OFFSET, 0x0);
	WriteMcReg8(McRegsBase, DDR3_INIT_START_OFFSET, 0x1);
	WaitInitDone(McRegsBase);

	WriteMcReg8(McRegsBase, 0x19, 0x1);
	WriteMcReg8(McRegsBase, 0x7, 0x0);
}
